#ifndef FRAME_H
#define FRAME_H

#include <vector>

#include <opencv2/opencv.hpp>
#include <DBoW3/DBoW3.h>
#include "../include/MapPoint.hpp"
#include "../include/Map.hpp"
#include "../include/Converter.hpp"
#include "../include/Imu.hpp"

#include <mutex>
using namespace std;
using namespace cv;

namespace ORBSLAM
{

#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64

    class ORBFeature;
    class MapPoint;
    class Bias;
    class Extrinsics;
    class Frame
    {
    private:
    public:
        Frame();

        // Copy constructor.
        // Frame(const Frame &frame);

        // Constructor for Monocular cameras.
        Frame(const Mat &im, ORBFeature *orbfeature, int feature_num_to_extract, double time_stamp);
        void assign_features_to_grid();
        bool Pos_in_grid(const KeyPoint &kp, int &row, int &col);
        vector<int> Get_candidate_points_to_match(const Point2f &vp_prematched, int window_size, int min_level, int max_level);
        void Set_pose(const Mat &Tcw);
        bool Is_inframe_field(MapPoint *p_mpt, float cos_value_th);
        void Set_preintegration(PreIntegrationData preintegrationdata_lastframe,
                                PreIntegrationData preintegrationdata_lastkeyframe);

        Mat m_image;
        int mn_feature_num;
        vector<KeyPoint> mv_orb_keypoints;
        Mat mm_descriptors;
        vector<KeyPoint> mv_orb_unkeypoints;
        vector<MapPoint *> mvp_mappoints;
        vector<bool> mvb_outline;

        Bias m_bias;
        Extrinsics m_extrinsics;
        double m_time_stamp;
        PreIntegrationData m_preintegrationdata_lastframe;
        PreIntegrationData m_preintegrationdata_lastkeyframe;

        static long unsigned int sm_frame_curid;
        long unsigned int mn_frame_id;

        // int mn_mpts_num;

        vector<int> mv_grid[FRAME_GRID_COLS][FRAME_GRID_ROWS];

        static float mf_box_minx;
        static float mf_box_miny;
        static float mf_box_maxx;
        static float mf_box_maxy;

        static float mf_grid_element_width_inv;
        static float mf_grid_element_height_inv;

        static bool mb_initial_image_bound;

        static vector<double> mvd_scale_factor;
        static vector<double> mvd_inv_scale_factor;
        static vector<double> mvd_level_sigma2;
        static vector<double> mvd_inv_level_sigma2;
        static int mi_nlevel;
        static double md_scale_factor;

        Matx31f Get_tcw_();
        Matx31f Get_twc_();
        Matx33f Get_Rwc_();
        Matx33f Get_Rcw_();
        Matx44f Get_Tcw_();
        Matx44f Get_Twc_();

        Mat Get_tcw();
        Mat Get_twc();
        Mat Get_Rwc();
        Mat Get_Rcw();
        Mat Get_Tcw();
        Mat Get_Twc();

    public:
        Mat mm_Tcw;
        Mat mm_Rcw;
        Mat mm_tcw;
        Mat mm_Twc;
        Mat mm_Rwc;
        Mat mm_twc;

        Matx44f mm_Tcw_;
        Matx33f mm_Rcw_;
        Matx31f mm_tcw_;
        Matx44f mm_Twc_;
        Matx33f mm_Rwc_;
        Matx31f mm_twc_;

    public:
        DBoW3::BowVector mv_bowvector;
        DBoW3::FeatureVector mv_featurevector;

        KeyFrame *mp_ref_keyframe;

        map<unsigned long, Point2f> mmap_mappoints_to_project;

        // mutex mmutex_frame_pose;
        shared_ptr<mutex> mp_mutex_frame_pose{make_shared<mutex>()};
    };

    class KeyFrame : public Frame
    {
    private:
        bool mb_bad;
        Map *mp_map;

    public:
        KeyFrame(Frame &frame);
        KeyFrame();
        void Init_keyframe();

        // void Compute_bow();
        void Add_mappoint(MapPoint *p_mappoint, const int &idx);
        void Update_connections();
        set<MapPoint *> Get_set_mappoints();
        float Compute_scene_median_depth(int q);
        int Tracked_mappoints(const int &minobs);
        void Set_pose(const Mat &Tcw);
        vector<MapPoint *> Get_vect_mappoints();
        void Set_map(Map *p_map);
        void Add_connection(KeyFrame *pKF, int weight);
        void Add_child(KeyFrame *pKF);
        void Update_best_covisibles();
        void Update_best_covisibles(KeyFrame *p_keyframe_culled);

        bool Is_bad();
        Map *Get_map();
        vector<KeyFrame *> Get_best_covisibility_keyframe(const int &n);
        vector<KeyFrame *> Get_all_covisibility_keyframe();
        vector<KeyFrame *> Get_covisible_by_weight(const int &w);
        set<KeyFrame *> Get_child();
        KeyFrame *Get_parent();

        set<KeyFrame *> Get_connect_keyframe();

        int Sum_mappoints_by_obsnum(const int nminobs);

        void Erase_mappoint_match(const int &idx);
        void Erase_mappoint_match(MapPoint *p_mpt);

        MapPoint *Get_mappoint(const int &idx);

        bool Is_in_image(const float &x, const float &y);

        vector<int> Get_feature_inarea(const float &x, const float &y, const float &r);

        void Replace_mappoint_match(const int &idx, MapPoint *p_mpt);

        void Set_bad_flag();

        void Erase_connection(KeyFrame *p_keyframe);

        int Get_weight(KeyFrame *p_keyframe);

        void Change_parent(KeyFrame *p_keyframe);

        void Erase_child(KeyFrame *p_keyframe);

        void Set_as_ref_keyframe(bool flag);

        bool Used_as_ref_keyframe();

        void Set_not_erase();
        void Set_erase();

        void Add_loopedge(KeyFrame *p_keyframe);
        set<KeyFrame *> Get_loop_edge();

        bool Has_children(KeyFrame *p_keyframe);

        static bool Lid(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2)
        {
            return p_keyframe1->mn_keyframe_id < p_keyframe2->mn_keyframe_id;
        }
        static bool Weight_comp(int a, int b)
        {
            return a > b;
        }

        Matx31f Get_tcw_();
        Matx31f Get_twc_();
        Matx33f Get_Rwc_();
        Matx33f Get_Rcw_();
        Matx44f Get_Tcw_();
        Matx44f Get_Twc_();

        Mat Get_tcw();
        Mat Get_twc();
        Mat Get_Rwc();
        Mat Get_Rcw();
        Mat Get_Tcw();
        Mat Get_Twc();

        KeyFrame *mp_keyframe_pre;
        KeyFrame *mp_keyframe_next;

        static long unsigned int sm_keyframe_curid;
        long unsigned int mn_keyframe_id;

        map<KeyFrame *, int> mmap_keyframe_connection_weight;
        vector<KeyFrame *> mvp_ordered_keyframe;
        vector<int> mv_ordered_weight;
        vector<pair<KeyFrame *, int>> mv_ordered_keyframe_weight;

        bool mb_current_place_recognition;

        bool mb_first_connect;

        KeyFrame *mp_parent;
        set<KeyFrame *> msp_children;

        Mat mm_Tcw_gba;
        unsigned long mn_loopKF_gba;

        Mat mm_T_cur_parent;

        unsigned long mn_localkeyframe_tmpid;

        int mn_mpts_num;

        bool mb_not_erase;
        bool mb_to_be_erase;

        bool mb_used_as_ref_keyframe;

        int mn_relocal_worlds;
        float mf_relocal_score;
        float mf_place_recognition_score;

        unsigned long mn_mark_fuse_id;
        unsigned long mn_mark_localba_id;
        unsigned long mn_mark_localba_fixed_id;
        unsigned long mn_relocal_query;
        unsigned long mn_place_recognition_query;
        int mn_place_recognition_worlds;

        set<KeyFrame *> msp_loop_edge;

    protected:
        mutex mmutex_feature;
        mutex mmutex_pose;
        mutex mmutex_connection;
        mutex mmutex_map;
        mutex mmutex_member;
    };

}

#endif
